// SEE END OF FILE FOR LICENSE TERMS

/****************************************************************************
 InterfaceTest -
*****************************************************************************/

#include "p30F6010A.h"

//	main program for testing the IMU.
//	Select device options:

_FOSC(CSW_FSCM_OFF & XT_PLL4);		// 16MHz clock with 4MHz crystal or resonator
_FWDT( WDT_OFF ) ;				// no watchdog timer
//_FWDT(WDT_ON & WDTPSA_8 & WDTPSB_12);
_FBORPOR( 	PBOR_OFF & // brown out detection off
			//PBOR_ON & // brown out detection on
			//BORV_27 & // brown out triggered at 2.7V
			//PWRT_64 & // powerup timer at 64ms
			MCLR_EN //&  // enable MCLR
			// RST_PWMPIN & // pwm pins as pwm
			//PWMxH_ACT_HI & // PWMH is active high
			//PWMxL_ACT_HI
					) ; // PMWL is active high
_FGS( CODE_PROT_OFF ) ; // no protection
//_FICD( 0xC003 ) ;		// normal use of debugging port
_FICD( ICS_PGD ) ;		// normal use of debugging port

#define SERVO_MAX 	2000
#define SERVO_MIN 	1000
#define SERVO_STEP 	5

#define SERVO_CENTRE 	((SERVO_MAX + SERVO_MIN) >> 1)
#define SERVO_START 	SERVO_CENTRE

#define FRAME_SIZE	25000

unsigned int servoPosition 	= SERVO_START;

unsigned int servoDirection = 1;

unsigned int PWMState 		= 0;


int main (void)
{	
	//	Initialize all modules and turn on the interrupts.
//	defaultCorcon = CORCON;

	// Timer 3 is used to measure time spent per second in interrupt routines
	// which enables the calculation of the CPU loading.
	TMR3 = 0 ;					// initialize timer'
	PR3	= FRAME_SIZE;			// Set frame timeout time
	T3CONbits.TCKPS = 1 ;		// prescaler = 4 option
	T3CONbits.TCS = 0 ;	    	// use the XTAL/PLL to drive the clock
	IEC0bits.T3IE = 1 ;			// enable the interrupt
	T3CONbits.TON = 1 ;			// turn on timer 3

	TRISA = 0;
	TRISB = 0;
	TRISC = 0;
	TRISD = 0;
	TRISE = 0;
	TRISF = 0;
	TRISG = 0;

// NOTE: Change TRISF and TRISG to =0x01 when a CAN trx is attached.

	SRbits.IPL = 0 ;	// turn on all interrupt priorities
	
	while (1)			//  nothing else to do...entirely interrupt driven
	{
	}
	return 0 ;
}

//  process T3 interrupt
// Timer 3 is used to find the end of receiver frame.
// Any received servo PWM signal will reset this timer
// When a full frame has been received, T3 is triggered before the timer finishes.
// During normal operation, this interrupt should never happen due to timeout.
void __attribute__((__interrupt__,__no_auto_psv__)) _T3Interrupt(void) 
{
	switch(PWMState)
	{
	case 0:
		PR3 = servoPosition >> 1;
		PWMState = 1;

		LATA	=	0b0100001000000000;
		LATB	=	0b0010101011010101;
		LATC	=	0b0100000000000010;
		LATD	=	0b0010010110101011;
		LATE	=	0b0000001001010101;
		LATF	=	0b0000000101011001;
		LATG	=	0b0000001101000110;
		break;
	case 1:
	//	PR3 = servoPosition >> 1;
		PR3 = ((SERVO_MAX + SERVO_MIN - servoPosition ) >> 1);
		PWMState = 2;

		LATA	=	~LATA;
		LATB	=	~LATB;
		LATC	=	~LATC;
		LATD	=	~LATD;
		LATE	=	~LATE;
		LATF	=	~LATF;
		LATG	=	~LATG;
		break;
	case 2:
		PR3 = (FRAME_SIZE >> 1) - SERVO_CENTRE;
		PWMState = 0;

		LATA	=	0;
		LATB	=	0;
		LATC	=	0;
		LATD	=	0;
		LATE	=	0;
		LATF	=	0;
		LATG	=	0;

		if(servoDirection == 0)
		{
			servoPosition -= SERVO_STEP;
			if(servoPosition <= SERVO_MIN)
				servoDirection = 1;
		}
		else
		{
			servoPosition += SERVO_STEP;
			if(servoPosition >= SERVO_MAX)
				servoDirection = 0;
		}
	}
	
	ClrWdt();
	IFS0bits.T3IF = 0 ;			// clear the interrupt
	
	return ;
}



/****************************************************************************/
// This is part of the servo and radio interface software
//
// ServoInterface source code
//	http://code.google.com/p/rc-servo-interface
//
// Copyright 2010 ServoInterface Team
// See the AUTHORS.TXT file for a list of authors of ServoInterface.
//
// ServoInterface is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// ServoInterface is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License 
// along with ServoInterface.  If not, see <http://www.gnu.org/licenses/>.
//
// Many parts of ServoInterface use either modified or unmodified code
// from the MatrixPilot pilot project.
// The project also contains code for modifying MatrixPilot to operate
// with ServoInterface.
// For details, credits and licenses of MatrixPilot see the AUTHORS.TXT file.
/****************************************************************************/
